﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;
using Microsoft.Xna.Framework;

namespace SpaceFight.Units.Steering
{
	class Align : SteeringBehavior
	{
		private SteeringInfo.InfoType m_neededInfo = SteeringInfo.InfoType.Target                |
		                                             SteeringInfo.InfoType.MaxAngularAcceleration|
		                                             SteeringInfo.InfoType.MaxAngularSpeed       |
		                                             SteeringInfo.InfoType.AlignTargetRadius     |
		                                             SteeringInfo.InfoType.AlignSlowRadius       |
		                                             SteeringInfo.InfoType.AlignTimeToTarget;
		
		public override void GetSteering(IUnit p_unit, SteeringOutput p_steeringOutput)
		{
			if(HasValidInput(p_steeringOutput, p_unit, m_neededInfo) == false)
			{
				return;
			}
			
			// get the naive direction to the target
			Quaternion rotation = p_unit.Orientation;
			rotation.Conjugate();
			rotation = rotation * p_unit.SteeringInfo.Target.Orientation;

			GetSteering(ref rotation, ref p_unit, p_steeringOutput);
		}

		protected void GetSteering(ref Quaternion rotation, ref IUnit p_unit,
		                           SteeringOutput p_steeringOutput)
		{
			// to angle axis representation
			float angle = QuaternionHelpers.Angle(ref rotation);
			Vector3 axis = QuaternionHelpers.Axis(ref rotation);

			// wrap
			if (Math.Abs(angle) > MathHelper.Pi) angle = -angle;
			float rotationSize = Math.Abs(angle);

			//check if we are there return no streering
			if (rotationSize < p_unit.SteeringInfo.AlignTargetRadius) return;


			float targetRotationSpeed;

			// Outside the slow radius
			if (rotationSize > p_unit.SteeringInfo.AlignSlowRadius)
			{
				// fully accelerate
				targetRotationSpeed = p_unit.SteeringInfo.MaxAngularSpeed;
			}
			else
			{
				// calculate scaled speed
				targetRotationSpeed = p_unit.SteeringInfo.MaxAngularSpeed * rotationSize /
									  p_unit.SteeringInfo.AlignSlowRadius;
				Debug.Assert(float.IsNaN(targetRotationSpeed) == false);
			}

			// the final target rotation combines speed (already in the variable and direction
			targetRotationSpeed *= angle / rotationSize;
			Debug.Assert(float.IsNaN(targetRotationSpeed) == false);

			// Acceleration tries to get to the target rotationSpeed
			p_steeringOutput.Angular = p_unit.AngularVelocity;
			p_steeringOutput.Angular.Conjugate();
			p_steeringOutput.Angular = p_steeringOutput.Angular * Quaternion.CreateFromAxisAngle(axis, targetRotationSpeed);

			// clip speeding
			QuaternionHelpers.Clamp(ref p_steeringOutput.Angular, p_unit.SteeringInfo.MaxAngularAcceleration);
		}
	}
}
